

#include "rviz_custom_plugins/displays/bag_map_display.h"
#include <rosbag2_cpp/reader.hpp>
#include <rclcpp/serialization.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rviz_common/load_resource.hpp>
#include <tf2/LinearMath/Transform.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <tf2_sensor_msgs/tf2_sensor_msgs.hpp>
#include <pcl_conversions/pcl_conversions.h>
#include <rviz_common/view_manager.hpp>
#include "rviz_custom_plugins/utils/project_manager.h"
#include <rviz_common/logging.hpp>


using rviz_custom_plugins::utils::ProjectManager;


namespace rviz_custom_plugins
{


namespace displays
{


BagMapDisplay::BagMapDisplay() : point_cloud_common_(new PointCloudCommon(this)), mapLoader(nullptr)
{
    // frame_property_ = new rviz_common::properties::TfFrameProperty(
    //     "Reference Frame", rviz_common::properties::TfFrameProperty::FIXED_FRAME_STRING,
    //     "The TF frame this grid will use for its origin.",
    //     this, 0, true);
    // frame_property_->hide();

    selectable_property_ = new rviz_common::properties::BoolProperty(
        "Selectable", true,
        "Whether or not the points in this point cloud are selectable.",
        this, nullptr, this);
    // selectable_property_->hide();

	size_property_ = new rviz_common::properties::IntProperty(
		"Pcd Size", 0,
		"Point CLoud Size",
        this, nullptr);

	point_size_property_ = new rviz_common::properties::IntProperty(
		"Point Size", 0,
		"Point Size",
        this, nullptr);


	for (int index = 0 ; index < numChildren(); index++ ) {
    	Property* childPro = childAtUnchecked(index);
        // if(childPro != size_property_) {
        //     childPro->hide();
        // }
		if (childPro && childPro->getName() == "Decay Time") {
			childPro->setValue(9999);
		}
  	}

    pclPcdPoint = pcl::PointCloud<pcl::PointXYZINormal>::Ptr(new pcl::PointCloud<pcl::PointXYZINormal>());
    pclPcdPointCache = pcl::PointCloud<pcl::PointXYZINormal>::Ptr(new pcl::PointCloud<pcl::PointXYZINormal>());

    mapNotifier = BagMapNotifier::getInstance();

    ProjectManager* projectManager = ProjectManager::getInstance();

    connect(mapNotifier, &BagMapNotifier::pointCloudLoaded, this, &BagMapDisplay::handlePointCloudLoaded);
    connect(mapNotifier, &BagMapNotifier::pointCloudFiltered, this, &BagMapDisplay::handlePointCloudFiltered);

    connect(projectManager, &ProjectManager::projectCreated, this, &BagMapDisplay::handleProjectLoaded, Qt::DirectConnection);
    connect(projectManager, &ProjectManager::projectOpened, this, &BagMapDisplay::handleProjectLoaded, Qt::DirectConnection);
    connect(projectManager, &ProjectManager::projectSaved, this, &BagMapDisplay::handleProjectSaved, Qt::DirectConnection);
    connect(projectManager, &ProjectManager::projectClosed, this, &BagMapDisplay::handleProjectClosed, Qt::DirectConnection);
    // qRegisterMetaType<sensor_msgs::msg::PointCloud2::SharedPtr>("sensor_msgs::msg::PointCloud2::SharedPtr");

}


BagMapDisplay::~BagMapDisplay() = default;


void BagMapDisplay::onInitialize()
{
	rosNode = context_->getRosNodeAbstraction();
	// PC2RDClass::onInitialize();
	point_cloud_common_->initialize(context_, scene_node_);
}


void BagMapDisplay::load(const Config & config) 
{
    Display::load(config);

	// QString mapDir;
	// config.mapGetString("MapDir", &mapDir);
	// loadMap(mapDir);
}


bool BagMapDisplay::loadMap(QString& mapDir)
{
	if(mapLoader) {
		return false;
	}

	mapLoader = new BagMapLoader(mapDir);
    connect(mapLoader, &QThread::finished, mapLoader, &QThread::deleteLater);

	// connect(mapLoader, &BagMapLoader::loadMapFinished, this, &BagMapDisplay::handleLoadMapFinished);
	mapLoader->start();

	RCLCPP_INFO(rosNode.lock()->get_raw_node()->get_logger(), "load map started");
	return true;
}


void BagMapDisplay::handlePointCloudLoaded(pcl::PointCloud<pcl::PointXYZINormal>::Ptr pcdPointNormal)
{
    pclOriginPcdPoint = pcdPointNormal;
    pclPcdPointCache = pcdPointNormal;
}


void BagMapDisplay::handlePointCloudFiltered(pcl::PointCloud<pcl::PointXYZINormal>::Ptr pcdPointNormal)
{
    pclPcdPointCache = pcdPointNormal;
}


void BagMapDisplay::setStatus(rviz_common::properties::StatusProperty::Level level, const QString & name, const QString & text)
{
    // do nothing
}


void BagMapDisplay::update(float wall_dt, float ros_dt)
{
    if(pclPcdPoint != pclPcdPointCache)
    {
        pclPcdPoint = pclPcdPointCache;
        sensor_msgs::msg::PointCloud2::SharedPtr rosPcd = std::make_shared<sensor_msgs::msg::PointCloud2>();
        pcl::toROSMsg(*pclPcdPoint, *rosPcd);
        rosPcd->header.frame_id = "map";

        point_cloud_common_->reset();
        point_cloud_common_->addMessage(rosPcd);
        size_property_->setInt(size_property_->getInt() + 1);
        point_size_property_->setInt(rosPcd->width * rosPcd->height);
    }
    point_cloud_common_->update(wall_dt, ros_dt);
}


void BagMapDisplay::reset()
{
  // PC2RDClass::reset();
  point_cloud_common_->reset();
}


void BagMapDisplay::onDisable()
{
  // PC2RDClass::onDisable();
  point_cloud_common_->onDisable();
}


void BagMapDisplay::handleProjectLoaded(QSharedPointer<QVariantMap> projectData)
{
    QString projectDir = (*projectData)["projectDir"].toString();
    QString mapDir = projectDir + "/map";
    // {
    //     std::ostringstream sstm;
    //     sstm << " map dir: " << mapDir.toStdString();
    //     RVIZ_COMMON_LOG_INFO(sstm.str().c_str());
    // }
    loadMap(mapDir);
}


void BagMapDisplay::handleProjectSaved(QSharedPointer<QVariantMap> projectData)
{

}


void BagMapDisplay::handleProjectClosed(QSharedPointer<QVariantMap> projectData)
{
    if(pclPcdPointCache) {
        pclPcdPointCache->clear();
        pclPcdPointCache = pcl::PointCloud<pcl::PointXYZINormal>::Ptr(new pcl::PointCloud<pcl::PointXYZINormal>);
    }

    if(pclOriginPcdPoint) {
        pclOriginPcdPoint->clear();
    }
    
}


}


}



#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(rviz_custom_plugins::displays::BagMapDisplay, rviz_common::Display)
